function tvxfhg = convert_pos_to_time_discrete(state, QsInfo)
% CONVERT_POS_TO_TIME_DISCRETE 将按位置离散的矩阵转换为按时间离散的矩阵
% 输入:
%   state     - 位置离散的状态矩阵（由 speedplan_cruise 生成）
%   QsInfo    - 包含以下字段的优化参数结构体:
%               .t_now    - 当前时间
%               .step_t   - 时间步长
%               .set_time - 设定时间
%               .t_err    - 允许的时间误差
% 输出:
%   tvxfhg    - 按时间离散的矩阵，列顺序为: 时间, 速度, 位置, 力, 高度, 坡度

% 提取所需列并构造位置离散矩阵
% 列顺序: 位置, 速度, 时间, 力差值, 高度, 坡度
xvtfhg = [state(:,1:3) ...
          state(:,4)-state(:,5) ...
          state(:,13) ...
          state(:,11)];
xvtfhg(:,3) = xvtfhg(:,3) + (QsInfo.t_now-1)*ones(length(xvtfhg),1);
% 生成时间向量
td_ = QsInfo.t_now : QsInfo.step_t : (QsInfo.t_now + QsInfo.set_time + QsInfo.t_err);

% 进行线性插值转换
x_new = interp1(xvtfhg(:,3), xvtfhg(:,1), td_, 'linear');  % 位置插值
v_new = interp1(xvtfhg(:,3), xvtfhg(:,2), td_, 'linear');  % 速度插值
f_new = interp1(xvtfhg(:,3), xvtfhg(:,4), td_, 'linear');  % 力插值
h_new = interp1(xvtfhg(:,3), xvtfhg(:,5), td_, 'linear');  % 高度插值
grd_new = interp1(xvtfhg(:,3), xvtfhg(:,6), td_, 'linear');% 坡度插值

% 组合时间离散矩阵
tvxfhg = [td_' v_new' x_new' f_new' h_new' grd_new'];

% 处理插值后的NaN值（使用末值填充）
nan_mask = isnan(tvxfhg(:,2));  % 检测速度列的NaN
if any(nan_mask)
    last_valid = xvtfhg(end, [2 1 4 5 6]);  % 获取最后一个有效值
    tvxfhg(nan_mask, 2:end) = repmat(last_valid, sum(nan_mask), 1);
end
end